//
//  optimization.cpp
//  template4
//
//  Created by Yuchen Liu on 6/5/12.
//  Copyright 2012 UCLA. All rights reserved.
//

#include <iostream>
#include "optimization.h"

COptimization::COptimization()
{
    
}

ParamSet* COptimization::initialize(int _numController, 
                                   double _duration, 
                                   double _timestep, 
                                   const double* _ctrUpper, 
                                   const double* _ctrLower,
                                   const double* _initVal)
{
    ParamSet *param = new ParamSet();
    
    numController = _numController;
    duration = _duration;
    timestep = _timestep;
    
    ctrUpper = new double[numController];
    ctrLower = new double[numController];
    for(int i = 0; i < numController; i++)
    {
        ctrUpper[i] = _ctrUpper[i];
        ctrLower[i] = _ctrLower[i];
    }

    
    numTimepoints = (int)(duration / timestep);
    
    param->numTimepoints = numTimepoints;
    param->numController = numController;
    param->u = new double*[numTimepoints];
    
    param->u[0] = new double[numController];
    for(int i = 0; i < numController; i++)
    {
        param->u[0][i] = _initVal[i];
    }
    
    for(int i = 1; i < numTimepoints; i++)
    {
        param->u[i] = new double[numController];
        for(int j = 0; j < numController; j++)
        {
            //double randVal = rand() % ((int)((ctrUpper[j] - ctrLower[j]) * 1000)) / 1000.0 + ctrLower[j];
            double randVal = (double)(rand() % 1000) / 1000.0;
            param->u[i][j] = randVal;
        }
        
    }
    
    return param;
}

void COptimization::update(ParamSet *curParam, double heat, double range)
{
    //ParamSet nxtParam;
    
    //nxtParam.numController = curParam.numController;
    //nxtParam.numTimepoints = curParam.numTimepoints;
    
    int left = (int)(heat * numTimepoints * numController);
    if(left == 0) left = 1;
    left = 1;
    srand(time(NULL));
    while(left > 0)
    {
        int selTP = rand() % numTimepoints;
        int selCT = rand() % numController;
        //double adjust = rand() % ((int)((ctrUpper[selCT] - ctrLower[selCT]) * range * 10000)) / 10000.0;
        double adjust = (double)(rand() % 10000) / 10000.0 * range;
        int pos = rand() % 2 * 2 - 1;
        
        curParam->u[selTP][selCT] += adjust * pos;
        
//        if(curParam->u[selTP][selCT] > ctrUpper[selCT])
//            curParam->u[selTP][selCT] = ctrUpper[selCT];
//        else if (curParam->u[selTP][selCT] < ctrLower[selCT])
//            curParam->u[selTP][selCT] = ctrLower[selCT];

        if(curParam->u[selTP][selCT] > 1.0)
            curParam->u[selTP][selCT] = 1.0;
        else if(curParam->u[selTP][selCT] < 0.0)
            curParam->u[selTP][selCT] = 0.0;
        
        left--;
    }
}

bird_dof* COptimization::realize_bird_action(ParamSet param)
{
//    CEngine* engine = new CEngine();
//    
//    bird_prop bird;
//    bird.mass = 10.0;
//    
//    bird_status curStatus;
//    bird_dof curDof;
//    curStatus.posX = 0.0;
//    curStatus.posY = 0.0;
//    curStatus.posZ = 0.0;
//    curStatus.spdX = 5.0;
//    curStatus.spdY = 0.0;
//    curStatus.spdZ = 0.0;
//    
    bird_dof* series = new bird_dof[numTimepoints];
//    for(int i = 0; i < numTimepoints; i++)
//    {
//        bird_status nxtStatus;
//        nxtStatus.u = param.u[i];
//        nxtStatus.posX = curStatus.posX;
//        nxtStatus.posY = curStatus.posY;
//        nxtStatus.posZ = curStatus.posZ;
//        nxtStatus.spdX = curStatus.spdX;
//        nxtStatus.spdY = curStatus.spdY;
//        nxtStatus.spdZ = curStatus.spdZ;
//        
//        curDof = engine->bird_dof_by_status(nxtStatus);
//        bird_acc curAcc = engine->aerodynamic_force(curDof, bird);
//        engine->update_bird_status(nxtStatus, curAcc, timestep);
//        
//        curDof.trunk_pos_x = nxtStatus.posX;
//        curDof.trunk_pos_y = nxtStatus.posY;
//        curDof.trunk_pos_z = nxtStatus.posZ;
//        curDof.trunk_rot_x = nxtStatus.rotX;
//        curDof.trunk_rot_y = nxtStatus.rotY;
//        curDof.trunk_rot_z = nxtStatus.rotZ;
//        
//        series[i] = curDof;
//        
//        curStatus = nxtStatus;
//    }
    
    return series;
}

double COptimization::evaluate_objective_function(bird_status *series, const ParamSet *paramSet)
{
    double posDevSum = 0.0;
    double rotDevSum = 0.0;
    double energeSum = 0.0;
    
    for(int i = 0; i < numTimepoints + 1; i++)
    {
        //posDevSum += sqrt(series[i].trunk_pos_y * series[i].trunk_pos_y + series[i].trunk_pos_z * series[i].trunk_pos_z);
        posDevSum += sqrt(series[i].pos.x * series[i].pos.x + series[i].pos.y * series[i].pos.y + series[i].pos.z * series[i].pos.z);
    }
    
    for(int i = 1; i < numTimepoints; i++)
    {
        for(int j = 0; j < paramSet->numController; j++)
        {
            energeSum += (paramSet->u[i][j] - paramSet->u[i-1][j]) * (paramSet->u[i][j] - paramSet->u[i-1][j]);
        }
    }
    
        
//        if(i == 0)
//            continue;
//        else
//        {
//            bird_dof curDof = series[i];
//            bird_dof preDof = series[i-1];
//            
//            double innerprod = curDof.trunk_rot_x * preDof.trunk_rot_x
//                            +  curDof.trunk_rot_y * preDof.trunk_rot_y
//                            +  curDof.trunk_rot_z * preDof.trunk_rot_z;
//            double len1 = sqrt(curDof.trunk_rot_x * curDof.trunk_rot_x
//                               +curDof.trunk_rot_y * curDof.trunk_rot_y
//                               +curDof.trunk_rot_z * curDof.trunk_rot_z);
//            double len2 = sqrt(preDof.trunk_rot_x * preDof.trunk_rot_x
//                               +preDof.trunk_rot_y * preDof.trunk_rot_y
//                               +preDof.trunk_rot_z * preDof.trunk_rot_z);
//            rotDevSum += fabs(acos(innerprod / (len1 * len2)));
//            
//            energeSum += fabs(curDof.shoulder_sweep - preDof.shoulder_sweep) +
//                         fabs(curDof.lshoulder_twist - preDof.lshoulder_twist) + 
//                         fabs(curDof.lshoulder_dihedral - preDof.lshoulder_dihedral) + 
//                         fabs(curDof.lwrist_bend - preDof.lwrist_bend) + 
//                         fabs(curDof.lwrist_twist - preDof.lwrist_twist) + 
//                         fabs(curDof.lshoulder_twist - preDof.lshoulder_twist) + 
//                         fabs(curDof.lshoulder_dihedral - preDof.lshoulder_dihedral) + 
//                         fabs(curDof.lwrist_bend - preDof.lwrist_bend) + 
//                         fabs(curDof.lwrist_twist - preDof.lwrist_twist) + 
//                         fabs(curDof.tail_bend - preDof.tail_bend) +
//                         fabs(curDof.tail_twist - preDof.tail_twist);
//        }
//    }
//    
//    return posDevSum + rotDevSum + energeSum;
        
    return posDevSum + energeSum / (double)(paramSet->numTimepoints * paramSet->numController);
}
